#include <stdio.h>
#include <termios.h>//结构体头文件
#include <string.h>//bzero头文件
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "JY61P.h"
#include "pid.h" //包含pid计算函数的头文件
#include "pwm_ctrl.h" //包含pwm控制函数的头文件 
#include <stdint.h>
#include "gpio_only.h"
#include "gpio_dir.h"

int main(int argc, char *argv[]) {

    int fd;//串口设备
    int fd0;
    uint8_t buf[11]; // 用于存储从串口读取的数据
    int count;
    struct jy61p_data data; // 存储解析后的jy61p数据的结构体
    int rx_state = 0; // 接收状态机状态
    int rx_index = 0; // 接收数据索引
    uint8_t rx_byte; // 接收的字节
     uint8_t rx_byte1; // 接收的字节
    float duty = 200; //用于存储pid计算后的duty值
    char pwm2_path[100];
    int period ;//初始值
    int duty_cycle ;//初始值
    float change = 0;//初始值
    char period_str[32];
    char duty_str[32];
     uint8_t buf0[11]; // 用于存储从串口读取的数据
 
    gpioinit();//初始化gpio 
    gpioinit2();
    gpioinit3();

    //首先，判断该pwm2通道0是否已经导出用户空间
    sprintf(pwm2_path,"/sys/class/pwm/pwmchip0/pwm%s","0");

    if (access((pwm2_path),F_OK))//新面孔access函数
    {
        /* code */
        pwm2_export("0");//这里的argv[1]代表着传入的pwm通道pwm0
    }
    //串口1
    // 步骤一：打开串口设备
    fd = open("/dev/ttyS9", O_RDWR | O_NOCTTY | O_NDELAY);//uart9
    
    if (fd < 0) {
        printf("open error \n");
        return -1;//判断有无打开串口
    }

    // 步骤二：设置串口参数
    set_uart(fd, 9600, 8, 'N', 1);//波特率9600,数据位8,无校验，停止位1
    
    //串口2
    fd0 = open("/dev/ttytr", O_RDWR | O_NOCTTY | O_NDELAY);//uart9
    
    if (fd0 < 0) {
        printf("open error \n");
        return -1;//判断有无打开串口
    }
    set_uart(fd0, 9600, 8, 'N', 1);//波特率9600,数据位8,无校验，停止位1

    sleep(2);  // 等待设备初始化

    // 步骤三：循环读取jy61p数据*/
    
    while (1) {
      
        // 通过串口读取数据
        // 注意：这里假设每次读取一个字节，实际应用中可能需要
        if (read(fd, &rx_byte, 1) <= 0) {
            usleep(1000);
            continue;
        }
        
        if (rx_state == 0) {
            if (rx_byte == 0x55) {
                buf[0] = rx_byte;
                rx_state = 1;
                rx_index = 1;
            }
        }
        else if (rx_state == 1) {
            if (rx_byte == 0x53) {
                buf[1] = rx_byte;
                rx_state = 2;
                rx_index = 2;
            } else {
                rx_state = 0;
                rx_index = 0;
            }
        }
        // 接收状态机状态2表示接收数据阶段
        else if (rx_state == 2) {

            buf[rx_index++] = rx_byte;
            
            if (rx_index == 11) { // 接收到完整的帧
                
                uint8_t sum = 0;
                for (int i = 0; i < 10; ++i) sum += buf[i];
                if (sum == buf[10]) {
                   // memset(&data, 0, sizeof(data));

                    get_jy61p_data(buf, &data); // 引用帧解析
                    printf("Roll: %.2f°, Pitch: %.2f°, Yaw: %.2f°\n",
                        data.roll, data.pitch, data.yaw);
                        
                } else {
                    printf("校验失败，丢弃帧\n");
                }

              rx_state = 0;
                rx_index = 0;
            }
        }

        if (read(fd0, &rx_byte1, 1) <= 0) {
            usleep(1000);
            continue;
        }
        if (rx_byte1 == 1) {
                buf0[0] = rx_byte1;
                pid_init(0.2 , 0 , 0); 
            }
        if (rx_byte1 == 2) {
                buf0[0] = rx_byte1;
                pid_init(0.15 , 0 , 0); 
            }
        if (rx_byte1 == 1) {
                buf0[0] = rx_byte1;
                pid_init(0.09 , 0 , 0); 
            }
        
        
    
    //pid参数初始化
   // pid_init(0.1 , 0 , 0); 

    //按键中断检测
    gpio_interrupt();
    gpio_interrupt2();
    gpio_interrupt3();
    gpio_interrupt4();

    if (KeyNum == 1) {
        // 电机正转，设置GPIO138为高电平
         // 将GPIO138设置为高电平
        gpio_init138("0");
        //正转
        pwm2_ctrl("enable","1");  //使能
        printf("Motor is running forward\n");
    } 
    else if(KeyNum == 2)
    {
        gpio_init138("1");//正转
        pwm2_ctrl("enable","1");  //使能
        
        printf("Motor is running backward\n");
    }
    else if (KeyNum == 0||KeyNum == 3 ) {
        // 电机停止，通过设置PWM占空比为100%来停止
        pwm2_ctrl("enable","0");  //使能
        printf("Motor is stopped\n");
    }   

    if(statement == 1)
    {
        duty = 200;

    }
    if(statement == 2)
    {
        duty = pid_calculate(data.pitch);

    }

   // duty = pid_calculate(data.pitch);//参数为传感器的pitch值，duty ∈ [160, 240]
    period = (int)(duty * 700);  // 转成纳秒，如 200 -> 7,000,000 ns, 240 -> 8,400,000 ns, 160 -> 5,600,000 ns
    duty_cycle = period * 0.5; // 占空比为50%，即周期的一半  

        printf("Change: %.2f\n",duty); 

        snprintf(period_str, sizeof(period_str), "%d", period);
        snprintf(duty_str, sizeof(duty_str), "%d", duty_cycle); 
    

        pwm2_ctrl("period",period_str); //周期
        pwm2_ctrl("duty_cycle",duty_str);  //占空比
      


  }

    //步骤四：关闭串口设备
   // close(fd);
     gpio_unexport("122");
    return 0;
#include "uart_tr.h"

 }